#include <Arduino.h>
#include <TCA9548A.h>
#include <U8g2lib.h>
#include <PSX.h>
#include <Adafruit_SPIDevice.h>
#define dataPin 10  // Brown wire
#define cmdPin 11   // Orange wire
#define attPin 12   // Yellow wire
#define clockPin 13 // Blue wire
#include <motor.h>
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif

#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif

#define SPIDEVICE_CS 10
Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS, 100000, SPI_BITORDER_MSBFIRST, SPI_MODE1);

// U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /* clock=*/SCL, /* data=*/SDA, /* reset=*/U8X8_PIN_NONE); // All Boards without Reset of the Display
TCA9548A I2CMux; // Address can be passed into the constructor
PSX psx;

PSX::PSXDATA PSXdata;
int PSXerror;
static const uint8_t Motor0 = 3;
static const uint8_t Motor1 = 4;
static const uint8_t Motor2 = 6;
static const uint8_t Motor3 = 9;

static const uint8_t DIR0 = 2;
static const uint8_t DIR1 = 5;
static const uint8_t DIR2 = 7;
static const uint8_t DIR3 = 8;

void psx_steup()
{
  //Setup the PSX library
  psx.setupPins(dataPin, cmdPin, attPin, clockPin, 5);
  psx.config(PSXMODE_ANALOG);
  // Setup serial communication
}
// void screen_steup()
// {
//   u8g2.begin();
//   u8g2.enableUTF8Print();
//   u8g2.clearBuffer();					// clear the internal memory
//   u8g2.setFont(u8g2_font_ncenB08_tr);	// choose a suitable font
//   u8g2.setCursor(0, 10);
//   u8g2.print("Hello Maymay!");
//   u8g2.sendBuffer();					// transfer internal memory to the display
//   delay(1000);
// }

/*
void draw_ps_state()
{
  u8g2.clearBuffer();	
  u8g2.setCursor(0, 10);
  u8g2.print("Hello Maymay!");
  

  // Check if the read was successful
  if(PSXerror == PSXERROR_SUCCESS) {
      // Print the joystick states
      
      u8g2.setCursor(0, 25);
      u8g2.println("L  ");
      u8g2.println("X    ");
      u8g2.println(PSXdata.JoyLeftX);
      u8g2.println("Y    ");
      u8g2.println(PSXdata.JoyLeftY);
      u8g2.setCursor(0, 35);
      u8g2.println("R  ");
      u8g2.println("X    ");
      u8g2.println(PSXdata.JoyRightX);
      u8g2.println("Y    ");
      u8g2.println(PSXdata.JoyRightY);	
      u8g2.setCursor(0, 45);
      //Print the button states
      if(PSXdata.buttons & PSXBTN_LEFT) {
        u8g2.println("Left, ");
      }
      if(PSXdata.buttons & PSXBTN_DOWN) {
        u8g2.println("Down, ");
      }
      if(PSXdata.buttons & PSXBTN_RIGHT) {
        u8g2.println("Right, ");
      }
      if(PSXdata.buttons & PSXBTN_UP) {
        u8g2.println("Up, ");
      }
      if(PSXdata.buttons & PSXBTN_START) {
        u8g2.println("Start, ");
      }
      if(PSXdata.buttons & PSXBTN_SELECT) {
        u8g2.println("Select, ");
      }
      if(PSXdata.buttons & PSXBTN_SQUARE) {
        u8g2.println("Square, ");
      }
      if(PSXdata.buttons & PSXBTN_CROSS) {
        u8g2.println("Cross, ");
      }
      if(PSXdata.buttons & PSXBTN_CIRCLE) {
        u8g2.println("Circle, ");
      }
      if(PSXdata.buttons & PSXBTN_TRIANGLE) {
        u8g2.println("Triangle, ");
      }
      if(PSXdata.buttons & PSXBTN_R1) {
        u8g2.println("R1, ");
      }
      if(PSXdata.buttons & PSXBTN_L1) {
        u8g2.println("L1, ");
      }
      if(PSXdata.buttons & PSXBTN_R2) {
        u8g2.println("R2, ");
      }
      if(PSXdata.buttons & PSXBTN_L2) {
        u8g2.println("L2, ");
      }
  } else {
    u8g2.println("No success reading data. Check connections and timing.");
  }
  u8g2.sendBuffer();
}

*/
void activate(int motor, int dir)
{
  if(dir == 1)
  {
    digitalWrite(2+motor*2, HIGH);
    digitalWrite(3+motor*2, LOW);
  }
  else if (dir == 2)
  {
    digitalWrite(2+motor*2, LOW);
    digitalWrite(3+motor*2, HIGH);
  }
  else
  {
    digitalWrite(2+motor*2, LOW);
    digitalWrite(3+motor*2, LOW);
  }

}
int count = 0;
void setup()
{
  psx_steup();
  pinMode(DIR0, OUTPUT);
  pinMode(DIR1, OUTPUT);
  pinMode(DIR2, OUTPUT);
  pinMode(DIR3, OUTPUT);
  pinMode(Motor0, OUTPUT);
  pinMode(Motor1, OUTPUT);
  pinMode(Motor2, OUTPUT);
  pinMode(Motor3, OUTPUT);
  digitalWrite(Motor0, LOW);
  digitalWrite(Motor1, LOW);
  digitalWrite(Motor2, LOW);
  digitalWrite(Motor3, LOW);
  digitalWrite(DIR0, LOW);
  digitalWrite(DIR1, LOW);
  digitalWrite(DIR2, LOW);
  digitalWrite(DIR3, LOW);
}
void loop()
{
  count++;
  // Read controller state
  
  delay(1);
  if (count > 5)
  {
    count = 0;
  }
  else
  {
    int dir = 2;
    PSXerror = psx.read(PSXdata);
    if(PSXdata.buttons & PSXBTN_L2)
    {
      dir = 1;
    }
    if(PSXdata.buttons & PSXBTN_LEFT) {
      activate(0, dir);
    }
    else
    {
      activate(0, 0);
    }
    if(PSXdata.buttons & PSXBTN_DOWN) {
      activate(1, dir);
    }
    else
    {
      activate(1, 0);
    }
    if(PSXdata.buttons & PSXBTN_RIGHT) {
      activate(2, dir);
    }
    else
    {
      activate(2, 0);
    }
    if(PSXdata.buttons & PSXBTN_UP) {
      activate(3, dir);
    }
    else
    {
      activate(3, 0);
    }
  }
}
